# -*- coding: utf-8 -*-
import json
import logging
import math
import os
import socket
import sqlite3
import struct
import subprocess
import sys
import threading
import time
import signal
import re
import serial
from threading import Thread

import modbus_tk.defines as cst
import modbus_tk.modbus_tcp as modbus_tcp
from modbus_tk.modbus_rtu import RtuServer
from periphery import GPIO
from serial import Serial

import config

'''
####
Date : 2024-4-29
Notes: VSR实体控制器--server端

Test: 通讯格式
####
'''

robot_logger=logging.getLogger("Controler-Driver")
robot_logger.setLevel(logging.DEBUG)

algorithm_path = "/home/cat/gm/algorithm_run/"

class Controler_Server():
    def __init__(self, host = "0.0.0.0", port = 502):
        super().__init__()
        # 获取本机的IP地址和名字
        host_name = socket.gethostname()
        self.host_add = socket.gethostbyname(host_name)
        self.host = host
        self.port = port

        self.server_tcp_init()

        self.vsrserver = VSR_Server()
        self.algo_th = Thread(target= self.vsrserver.run)
        self.algo_th.start()

        self.robot = robot_control()
        self.master = master_io()

        self.th_update = Thread(target= self.status_update)
        self.th_update.start()


        self.algorithm_th = Thread(target= self.run_algorithm)
        self.algorithm_th.start()

        self.rtu_th = Thread(target= self.run_process)
        self.rtu_th.start()




    def server_tcp_init(self,):
        try:
            # if self.check_port_conflict():
            #     print(f"Port {self.port} has been used!!!")
            #     return False
            self.server = modbus_tcp.TcpServer(port=self.port,address=self.host,
                                               timeout_in_sec=5)
            self.server.start()
            self.register_init()
        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def check_port_conflict(self,):
        try:
            ret = False
            sock = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
            result = sock.connect_ex(self.host,self.port) # 检测有没有服务将这个端口以及IP地址占用
            if result == 0:
                ret = True
            else:
                ret = False
            sock.close()
            return ret
        except Exception as e:
            robot_logger.error(e,exc_info=True)
    
    def register_init(self,):
        self.slave =self.server.add_slave(1)  # 服务器上创建1号从机
        self.slave.add_block("robot6",cst.HOLDING_REGISTERS,0,400) # 创建400个保持寄存器
        self.slave.add_block("fifo",cst.HOLDING_REGISTERS,1024,100) # 创建50个保持寄存器用于存储FIFO
        self.slave.add_block("motor",cst.HOLDING_REGISTERS,10000,50) # 创建50个保持寄存器用于存储电机状态
        self.slave.add_block("di",cst.COILS,1024,50) # 创建50个线圈寄存器用于存储di
        self.slave.add_block("do",cst.COILS,20000,50) # 创建50个线圈寄存器用于存储d0

    def int2float(self,a,b):
        f=0
        try:
            z0=hex(a)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z1=hex(b)[2:].zfill(4) #取0x后边的部分 右对齐 左补零
            z=z0+z1 #高字节在前 低字节在后
            f=struct.unpack('!f', bytes.fromhex(z))[0] #返回浮点数
        except BaseException as e:
            print(e)
        return f

    def float_to_int16s(self,f):
        # 将浮点数打包成32位二进制数据
        b = struct.pack('f', f)
        # 将32位二进制数据拆分成两个16位二进制数据
        i1, i2 = struct.unpack('HH', b)
        return i2, i1

    # 485RTU 线程管理
    def run_process(self,):
        rtu_process = None 
        algorithm_name = "rtu_server_online.py"
        file_path = algorithm_path + algorithm_name
        print("modbus_RTU 线程管理启动")
        while True:
            try:
                time.sleep(0.5)
                if self.robot.rut_en==1:
                    time.sleep(0.5)
                    if self.robot.rtu_slave>0:
                        if rtu_process is None:
                            rtu_process = subprocess.Popen(["python3",file_path])
                        elif rtu_process.poll() is not None:  # 程序运行结束
                            rtu_process = subprocess.Popen(["python3",file_path])
                        else:
                            pass
                elif self.robot.rut_en==0:
                    if rtu_process is None:
                        pass
                    elif rtu_process.poll() is None: # 程序在运行
                        rtu_process.kill()
                    else:
                        pass
            
            except Exception as e:
                robot_logger.error(e,exc_info=True)

    def run_algorithm(self,):
        print("视觉算法 线程管理启动")
        while True:
            try:
                time.sleep(0.5)
                num =   self.slave.get_values("robot6",181,1)[0]
                if num !=0:
                    match num:
                        case 1:
                            algorithm_name = "algorithm.py"

                        case 2:
                            algorithm_name = "circle.py"           
                        case 3:
                            algorithm_name = "shape.py"
                        case 4:
                            algorithm_name = "color_lab.py"

                        case 5:
                            algorithm_name = "color_hsv.py"

                        case 6:
                            algorithm_name = "ocr_test.py"           
                        case 7:
                            algorithm_name = "yolov8_detect.py"
                        case 8:
                            algorithm_name = "algorithm8.py"
                        case _:
                            algorithm_name = "algorithm.py"
                    file_path = algorithm_path + algorithm_name

                    self.algo_th  = subprocess.Popen(["python3",file_path])   
                    time.sleep(2)
                    while self.algo_th.poll() is None:
                        time.sleep(0.5)
                    self.slave.set_values("robot6",181,0)
            
            except Exception as e:
                robot_logger.error(e,exc_info=True)

    def status_update(self,):
        while True:
            try:
                time.sleep(0.005)

                # 更新主机DI的状态
                m_di = []
                for i in range (0,13):       # 共有13个DI
                    if self.master.get_DI(num=i+1) == True:
                        m_di.append(0)
                    else:
                        m_di.append(1)
                self.slave.set_values("robot6",70,m_di)   # 获取DI并且刷新到MODBUS表中
                time.sleep(0.005)

                # 更新机器人的状态
                status = []
                slider_status = []
                s = self.robot.status_global
                if s != None:
                    match s["state"]:
                        case "Idle":
                            status = status + [1]
                        case "Alarm":
                            status = status + [2]
                        case "Home":
                            status = status + [3]
                        case "Run":
                            status = status + [4]
                        case _:
                            status = status + [0]
                    ang = s["angle"]
                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status.append(a[0])
                        status.append(a[1])

                    ang = s["coordinate"]
                    for i in range(len(ang)):
                        a = self.float_to_int16s(ang[i])
                        status.append(a[0])
                        status.append(a[1])
                        if i ==6:
                            slider_status.append(a[0])
                            slider_status.append(a[1])
                    
                    status.append(self.robot.robot_type_number)     # 增加机器人的类型
                    status.append(self.vsrserver.connect_flag)  # 增加数字仿真服务状态
                    status.append(self.robot.rtu_slave)

                    self.slave.set_values("robot6",0,status)
                    self.slave.set_values("motor",10000,slider_status)# 这里是使用modbus进行通讯IOA 


            except Exception as e:
                robot_logger.error(e,exc_info=True)

    # 启动在线线程
    def online(self):
        file_path = '/home/cat/gm/controler/process_dis.py'
        try:

            if os.path.exists(file_path):
                self.online_p = subprocess.Popen(f'python3 {file_path}',shell=True,preexec_fn=os.setsid)
            else:
                raise Exception(f"No {file_path}")
        except Exception as e:
            print(e)
        # 启动在线线程

    def stop_process(self):  
        if self.online_p and self.online_p.poll() is None:  # 检查进程是否还在运行  
            try:  
                os.killpg(os.getpgid(self.online_p.pid), signal.SIGTERM)  # 发送SIGTERM信号到进程组  
            except ProcessLookupError:  
                pass  # 进程可能已经不存在了  

    def signal_handler(self, sig, frame):  
        self.stop_process()  
        # 这里可以添加更多的清理工作  

    def dorun(self,):
        self.online()
        signal.signal(signal.SIGINT, self.signal_handler) 
        Mdo_flag = [0,0,0,0,0]
        bo = 0x00
        while True:
            try:
                time.sleep(0.1)

                # 1轮询执行DI的状态
                Mdo_list = self.slave.get_values("robot6",86,8)

                for i in range(5):   # 两个电机口和三个LED灯
                    if Mdo_list[i] == Mdo_flag[i]:
                        pass
                    else:
                        Mdo_flag[i]= Mdo_list[i]
                        match Mdo_list[i]:
                            case 0:
                                self.master.set_LED(num=i,value=False)  # DO输出低电平
                            case 1:
                                self.master.set_LED(num=i,value=True)   # DO输出高电平


                # 2执行DO的值
                if Mdo_list[7] == 1:
                    self.master.set_DO(num=Mdo_list[5],value=Mdo_list[6])
                    self.slave.set_values("do",20000+Mdo_list[5],Mdo_list[6])  # 向IOA寄存器中写值
                    self.slave.set_values("robot6",93,0)   # 将MODBUS表中的使能清除掉

                config.set_value("BO0",self.master.bo_out)  



                # 3执行机器人的指令
                cmd = self.slave.get_values("robot6",49,2)
                if cmd[0] == 1:
                    P = []
                    motion = self.slave.get_values("robot6",31,18)
                    for i in range(0,14,2):
                        P.append(self.int2float(motion[i],motion[i+1]))
                    self.robot.move(P=P,speed=motion[17],coordinate=motion[14],is_relative=motion[15],mode=motion[16])
                    self.slave.set_values("robot6",49,0)
                
                # 复位等命令
                match cmd[1]:
                    case 0:
                        pass
                    case 1:
                        self.robot.home_1axis(1)
                        self.slave.set_values("robot6",50,0)
                        
                    case 2:
                        self.robot.home_1axis(2)
                        self.slave.set_values("robot6",50,0)
                        
                    case 3:
                        self.robot.home_1axis(3)
                        self.slave.set_values("robot6",50,0)
                        
                    case 4:
                        self.robot.home_1axis(4)
                        self.slave.set_values("robot6",50,0)
                    case 5:
                        self.robot.home_1axis(5)
                        self.slave.set_values("robot6",50,0)

                    case 6:
                        self.robot.home_1axis(6)
                        self.slave.set_values("robot6",50,0)

                    case 7:
                        self.robot.home_1axis(7)
                        self.slave.set_values("robot6",50,0)

                    case 8:
                        self.robot.home()
                        self.slave.set_values("robot6",50,0)

                    case 9:
                        self.robot.home_6axis_in_turn()
                        self.slave.set_values("robot6",50,0)

                    case 10:
                        self.robot.home_7axis()
                        self.slave.set_values("robot6",50,0)

                    case 11:
                        self.robot.unlock_all_axis()
                        self.slave.set_values("robot6",50,0)

                    case 12:
                        self.robot.stop()
                        self.slave.set_values("robot6",50,0)

                    case 13:
                        self.robot.go_to_zero()
                        self.slave.set_values("robot6",50,0)


                #4 执行485模式


            except Exception as e:
                robot_logger.error(e,exc_info=True)


class master_io():
    def __init__(self):
        self.gobal_do = 0

        self.A0 = 0
        self.A1 = 1
        self.A2 = 2
        self.A3 = 3
        self.A4 = 4
        self.A5 = 5
        self.A6 = 6
        self.A7 = 7

        self.B0 = 8
        self.B1 = 9
        self.B2 = 10
        self.B3 = 11
        self.B4 = 12
        self.B5 = 13
        self.B6 = 14
        self.B7 = 15      

        self.C0 = 16
        self.C1 = 17 
        self.C2 = 18
        self.C3 = 19 
        self.C4 = 20
        self.C5 = 21
        self.C6 = 22
        self.C7 = 23

        self.chip0 = "/dev/gpiochip0"  # chip0
        self.chip1 = "/dev/gpiochip1"  # chip1
        self.chip2 = "/dev/gpiochip2"  # chip2
        self.chip3 = "/dev/gpiochip3"  # chip3
        self.chip4 = "/dev/gpiochip4"  # chip4

        self.BUTTON1 = GPIO(self.chip1, self.A0, "in")  # 拨码按钮   
        self.BUTTON2 = GPIO(self.chip1, self.A1, "in")  # 离线按钮
        self.BUTTON3 = GPIO(self.chip1, self.A4, "in")  # 复位按钮

        self.DI_9 = GPIO(self.chip1, self.B2, "in")  # DI9 端口
        self.DI_8 = GPIO(self.chip1, self.B1, "in")  # DI8 端口

        self.DI_7 = GPIO(self.chip4, self.C3, "in")  # DI7 端口
        self.DI_6 = GPIO(self.chip4, self.C5, "in")  # DI6 端口
        self.DI_5 = GPIO(self.chip4, self.C2, "in")  # DI5 端口
        self.DI_4 = GPIO(self.chip3, self.B4, "in")  # DI4 端口
        self.DI_3 = GPIO(self.chip3, self.C1, "in")  # DI3 端口
        self.DI_2 = GPIO(self.chip3, self.B6, "in")  # DI2 端口
        self.DI_1 = GPIO(self.chip3, self.B5, "in")  # DI1 端口
        self.DI_0 = GPIO(self.chip1, self.B0, "in")  # DI0 端口

        self.Motor_0 = GPIO(self.chip3, self.B2, "out")  # 电机正转
        self.Motor_1 = GPIO(self.chip3, self.B3, "out")  # 电机反转
        self.LED2 = GPIO(self.chip3, self.A6, "out")  # 绿色
        self.LED3 = GPIO(self.chip3, self.A7, "out")  # 蓝色
        # self.LED4 = GPIO(self.chip4, self.C2, "out")  # 红色

        self.DO0 = GPIO(self.chip3, self.B1, "out")  # DO输出1
        self.DO1 = GPIO(self.chip3, self.B0, "out")  # DO输出2
        self.DO2 = GPIO(self.chip4, self.C6, "out")  # DO输出3
        self.DO3 = GPIO(self.chip4, self.C4, "out")  # DO输出4

        self.bo_out =0x00  

    def bo_count(self,BO,bo_num,value):# 定义一个按位计算的函数
        # BO 是一个8位的16进制数
        # bo_bit 是第几个IO
        # value 是DO的值
        # 将DO状态上传IOA服务器

        try:
            bit_num = 0b00000000
            if value ==0:
                match bo_num:
                    case 0 :
                        bit_num = 0b11111110
                    case 1 :
                        bit_num = 0b11111101

                    case 2 :
                        bit_num = 0b11111011
                    case 3 :
                        bit_num = 0b11110111
                    case 4 :
                        bit_num = 0b11101111
                    case 5 :
                        bit_num = 0b11011111
                    case 6 :
                        bit_num = 0b10111111
                    case 7 :
                        bit_num = 0b01111111
                BO  =  BO & bit_num
            
            elif value ==1:
                match bo_num:
                    case 0:
                        bit_num = 0b00000001
                    case 1:
                        bit_num = 0b00000010
                    case 2:
                        bit_num = 0b00000100
                    case 3:
                        bit_num = 0b00001000
                    case 4:
                        bit_num = 0b00010000
                    case 5:
                        bit_num = 0b00100000
                    case 6:
                        bit_num = 0b01000000
                    case 7:
                        bit_num = 0b10000000
                BO  =  BO | bit_num
            return BO            

        except Exception as e:
            print(e)

    def get_DI(self,num :int =1):
        try:
            value = -1

            match num:
                case 1:
                    value = self.BUTTON1.read()
                case 2:
                    value = self.BUTTON2.read()   
                case 3:
                    value = self.BUTTON3.read() 
                case 4:
                    value = self.DI_0.read() 
                case 5:
                    value = self.DI_1.read() 
                case 6:
                    value = self.DI_2.read() 
                case 7:
                    value = self.DI_3.read() 
                case 8:
                    value = self.DI_4.read() 
                case 9:
                    value = self.DI_5.read() 
                case 10:
                    value = self.DI_6.read() 
                case 11:
                    value = self.DI_7.read() 
                case 12:
                    value = self.DI_8.read() 
                case 13:
                    value = self.DI_9.read() 
              
            return value
        except Exception as e:
            robot_logger.error(e,exc_info=True) 

    # 这里是设置的四个灯的状态资源
    def set_LED(self,num,value=True):
        try:
            match num:
                case 0:
                    self.Motor_0.write(value)
                    if value:
                        self.bo_out = self.bo_count(self.bo_out,bo_num=6,value=1)
                    else:
                        self.bo_out = self.bo_count(self.bo_out,bo_num=6,value=0)
                case 1:
                    self.Motor_1.write(value)
                case 2:
                    self.LED2.write(value)
                case 3:
                    self.LED3.write(value)
                case 4:
                    pass
                    # self.LED4.write(value)


        except Exception as e:
            robot_logger.error(e,exc_info=True)

    #设置DO只需要调用这个函数
    def set_DO(self,num,value=1):
        try:
            value_bool = True
            if value == 0:
                value_bool =False
            elif value == 1:
                value_bool = True
            match num:
                case 0:
                    self.DO0.write(value_bool)
                case 1:
                    self.DO1.write(value_bool)
                case 2:
                    self.DO2.write(value_bool)
                case 3:
                    self.DO3.write(value_bool)

            self.bo_out = self.bo_count(self.bo_out,num,value=value)
        except Exception as e:
            robot_logger.error(e,exc_info=True)

class VSR_Server:
    def __init__(self) -> None:
        self.connect_flag = 0
        self.vsr_Server()
        self.robot = robot_control()

        self.robot_type = 0
        self.global_state = [1,1,1]
        ### IO 状态
        self.IO = [0,1,1,1,1,1,1,1]
        self.default_speed = 1000

        self.select_com_serial = False
        self.auto_flag = True
        self.threads = []

        self.lock_object = threading.RLock()

    def vsr_Server(self):
        self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.tcp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, True)

        config.set_value(name="BO0",value=0)
        config.set_value(name="BO1",value=0) #初始化16个DO 
        speed_value=(self.float2bytes(75.0))
        config.set_value("FO[0]",value=speed_value)
        f01=(self.float2bytes(75.0))
        config.set_value("FO[1]",value=f01)

        self.tcp_socket.bind(('0.0.0.0', 8090))
        self.tcp_socket.listen(5)

    def float2bytes(self,f):
    #将浮点数打包成32位2进制数据
        b=struct.pack('<f',f)        
        return b[0],b[1],b[2],b[3]      


    def run(self):  
        try:  
            while True:  
                self.client_socket, self.client_addr = self.tcp_socket.accept()
                print(f"Connected by {self.client_addr}")
                # client_socket.setblocking(False)
                robot_logger.info(f"Connected by {self.client_addr}")  
                # 为每个客户端启动一个新的线程  

                client_thread = threading.Thread(target=self.handle_client, args=(self.client_socket, self.client_addr))  
                client_thread.start()  
                self.threads.append(client_thread)  

        except KeyboardInterrupt:  
            robot_logger.info("Server is shutting down...")  
            # 尝试优雅地关闭所有线程和套接字  
            self.tcp_socket.close()  
            for thread in self.threads:  
                thread.join()  # 等待所有线程完成  
  
    def handle_client(self, client_socket, client_addr):  
        try:  
            while True:
                try:
                    recv_data = self.client_socket.recv(1024, 0x40) 
                    print(f"2024-7-8 recv_data is {recv_data}") 
                    if not recv_data: 
                        print("2024-7-8 recv_data is None") 
                        break  # Client disconnected
    
                    recv_str = recv_data.decode('utf-8')  
                    if "A-Mode" in recv_str:  
                        self.auto_flag = True
                        time.sleep(0.05)  
                        self.send_data_process()  
                    else:  
                        self.auto_flag = False
                        time.sleep(0.01)  
                        self.parse_data(recv_str) 
                
                except BlockingIOError as e:
                    recv_data = None
                    time.sleep(0.2)
                    self.send_data_process()
        
        finally:  
            client_socket.close()  
            robot_logger.info(f"Disconnected {client_addr}")  


    def parse_data(self, recv_data):
        speed = 2000
        index = recv_data.find('}')  
        if index != -1:  
            # 分割数据  
            # first_part = recv_data[:index+1]  
            second_part = recv_data[index+1:]  

        pattern_1 = r'"type":"([^"]*)","COM1":"([^"]*)"'
        pattern_2 = r'"type":"([^"]*)","COM2":"([^"]*)"'

       
        if "COM1" in second_part:
            print(f"--- success in --COM1-")
            match = re.search(pattern_1, second_part)
            self.select_com_serial = False
        else:
            print(f"--- success in ---COM2-")
            match = re.search(pattern_2, second_part)
            self.select_com_serial = True

        try:
            ## 检查是否是包含"type"和"C0M1"的JSON对象  
            if match:
                data_type = match.group(1)
                command = match.group(2)
                # print(f"data_type group1 is {data_type}, command is {command}") 

                gcode = f"M50\n"
                self.robot.serial_com1.write(gcode.encode('utf-8'))
                self.robot.serial_com1.write(b"\r\n")
                time.sleep(1)
                self.robot.serial_com2.write(gcode.encode('utf-8'))
                self.robot.serial_com2.write(b"\r\n")
                
                if data_type == 'M21':
                    if "up" in command:
                        gcode = f"!\n"
                        # time.sleep(1)
                        if self.select_com_serial:
                            time.sleep(1)

                            self.robot.serial_com2.write(gcode.encode('utf-8'))
                            self.robot.serial_com2.write(b"\r\n")
                        else:
                            self.robot.serial_com1.write(gcode.encode('utf-8'))
                            self.robot.serial_com1.write(b"\r\n")
                        time.sleep(0.5)
                        gcode = f"%\n"
                    elif "down" in command : 
                        self.lock_object.acquire()
                        s_data = self.robot.status_global
                        self.lock_object.release() 
                        # time.sleep(0.01)
                        angle_current = s_data["angle"]
                        angle_current = [int(i) for i in angle_current] 
                        ## 根据不同的命令执行不同的操作
                        match command:
                            case "1_+_down" :
                                gcode = "M21 G90 G01 X160 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "1_-_down" :
                                gcode = "M21 G90 G01 X-100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "2_+_down" :
                                gcode = "M21 G90 G01 X{} Y70 Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "2_-_down" :
                                gcode = "M21 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "3_+_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z60 A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "3_-_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z-120 A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "4_+_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A180 B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "4_-_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "5_+_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B29 C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "5_-_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B-199 C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "6_+_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C349 F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "6_-_down" :
                                gcode = "M21 G90 G01 X{} Y{} Z{} A{} B{} C-349 F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                        
                    else:
                        angle = json.loads(command)
                        self.robot.move(P = angle, coordinate=0)                 
                
                elif data_type == 'M20':
                    
                    if "up" in command:
                        gcode = f"!\n"
                        # time.sleep(1)
                        if self.select_com_serial:
                            time.sleep(1)
                            self.robot.serial_com2.write(gcode.encode('utf-8'))
                            self.robot.serial_com2.write(b"\r\n")
                        else:
                            self.robot.serial_com1.write(gcode.encode('utf-8'))
                            self.robot.serial_com1.write(b"\r\n")
                        time.sleep(0.5)
                        gcode = f"%\n"
                    elif "down" in command : 
                        self.lock_object.acquire()
                        s_data = self.robot.status_global
                        self.lock_object.acquire()
                        time.sleep(0.01)
                        angle_current = s_data["coordinate"]
                        angle_current = [int(i) for i in angle_current]          
                        ## 根据不同的命令执行不同的操作
                        match command:
                            case "1_+_down" :
                                gcode = "M20 G90 G01 X160 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "1_-_down" :
                                gcode = "M20 G90 G01 X-100 Y{} Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "2_+_down" :
                                gcode = "M20 G90 G01 X{} Y70 Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "2_-_down" :
                                gcode = "M20 G90 G01 X{} Y-35 Z{} A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "3_+_down" :
                                gcode = "M20 G90 G01 X{} Y{} Z60 A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "3_-_down" :
                                gcode = "M20 G90 G01 X{} Y{} Z-120 A{} B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "4_+_down" :
                                gcode = "M20 G90 G01 X{} Y{} Z{} A180 B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "4_-_down" :
                                gcode = "M20 G90 G01 X{} Y{} Z{} A-180 B{} C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "5_+_down" :
                                gcode = "M20 G90 G01 X{} Y{} Z{} A{} B29 C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "5_-_down" :
                                gcode = "M20 G90 G01 X{} Y{} Z{} A{} B-199 C{} F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "6_+_down" :
                                gcode = "M20 G90 G01 X{} Y{} Z{} A{} B{} C349 F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                            case "6_-_down" :
                                gcode = "M20 G90 G01 X{} Y{} Z{} A{} B{} C-349 F{}\n".format(angle_current[1],angle_current[2],\
                                        angle_current[3],angle_current[4],angle_current[5], speed)
                        
                        # self.robot.serial_com1.write(gcode.encode())
                    else:
                        angle = json.loads(command)
                        self.robot.move(P = angle, coordinate=0)                 

                elif data_type == 'cmd':
                    if command is not None:
                        match command:
                            case "restart":
                                gcode = f"$h\r\n"
                                print("复位操作")
                            
                            case "stop":
                                gcode = f"%\r\n"

                            case _:
                                pass

                # time.sleep(1)
                self.lock_object.acquire()
                if self.select_com_serial:
                    time.sleep(1)
                    self.robot.serial_com2.write(gcode.encode('utf-8'))
                    self.robot.serial_com2.write(b"\r\n")
                else:
                    self.robot.serial_com1.write(gcode.encode('utf-8'))
                    self.robot.serial_com1.write(b"\r\n")

                time.sleep(0.006)
                self.lock_object.release()

        except json.JSONDecodeError as e:  
            print(f"JSON解析错误: {e}")


    # 定义通讯协议格式  
    def build_packet(self,data_size, packet_type, message_body):  
        # 构建报头  
        header = json.dumps({  
            "data_size": data_size,  
            "type": packet_type  
        }).encode('utf-8')
        
        # 计算报头长度（4位bit，即半个字节，需要转换为字节长度）  
        header_length = len(header)
        
        # 构建帧头
        # frame_header = struct.pack('>H', header_length)
        frame_header = header_length.to_bytes(4, 'little')
        
        # 完整的数据包  
        packet = frame_header + header + message_body.encode('utf-8')
        return packet 

    # 定义消息体  
    def create_message_body(self,robot_type, state, com1, com2, rs485, com1Float, com2Float,io):  
        message_body = {  
            "robot_type": robot_type,  
            "state": state,  
            "com1": com1,  
            "com2": com2,  
            "RS485": rs485, 
            "com1Float": com1Float,
            "com2Float": com2Float,
            "IO": io  
        }
        data_str = json.dumps(message_body) + '\n'
        return data_str

    def robot_type_send(self):
        a = self.robot.robot_type_number 
        b = self.robot.robot_type_number_2
        c = "{}_{}".format(a, b)
        return c

    def send_data_process(self, ):
        try:
            if self.auto_flag:
                P = list()
                F = list()
                P.append(config.get_value("j0"))  
                P.append(config.get_value("j1"))
                P.append(config.get_value("j2"))
                P.append(config.get_value("j3"))
                P.append(config.get_value("j4"))
                P.append(config.get_value("j5"))
                
                F.append(config.get_value("j7"))
                F.append(config.get_value("j8"))
                F.append(config.get_value("j9"))
                F.append(config.get_value("j10"))
                
                if P is not None:
                    P = P
                else:
                    P = [0,0,0,0,0,0]
                
                if F is not None:
                    F = F
                else:
                    F = [0,0,0,0]
                

                robot_type = self.robot_type_send()
                state = self.global_state
                
                com1 = P
                com2 = F
                rs485 = ""
                com1Float = config.get_value("FO[1]")
                com2Float = config.get_value("FO[1]")
                io = self.IO
                
                ### 消息体
                message_body = self.create_message_body(robot_type, state, com1, com2, rs485, com1Float, com2Float, io)
                ### 消息体长度
                data_size = len(message_body)
                ### 控制器类型：实体控制器
                packet_type = "Physical" 
                ### 构建数据包 
                packet = self.build_packet(data_size, packet_type, message_body)
                print(f"2024---7-8----send_data is {packet}")
                self.client_socket.sendall(packet)
        
        except Exception as e:
            print(f"send_process data is error is {e}")


####
#com1 六轴机器人#com2 四轴机器人 
####
class robot_control:

    def __init__(self) -> None:
        # 定义一个端口列表  
        ports = ['/dev/ttyUSB0', '/dev/ttyUSB1']  
        
        # 尝试打开端口  
        self.serial_com2 = self.try_serial_ports(ports) 

        # self.serial_com2 = Serial(port="/dev/ttyUSB0",baudrate=115200)
        self.thread_update = Thread(target=self.task_com2,)
        self.thread_update.start()
        self.status_global_ = None

        self.serial_com1 = Serial(port="/dev/ttyS7",baudrate=115200)
        self.thread_update_ = Thread(target=self.task_com1,)
        self.thread_update_.start()
        self.status_global = None


        time.sleep(0.01)
        self.default_speed = 2000
        # 这里来表示机器人类型。0：代表无机器人，1:代表是六轴，2：代表是四轴
        self.robot_type_number = 0 
        self.robot_type_number_2 = 0

        # 定义RTU服务内容
        self.rtu_slave = 0
        self.rut_en = 0

        ########################数据库存储状态############################
        filename = 'ktr6.db'
        file_dir = "/home/cat/gm/data/"
        # # 使用系统分隔符连接目录和文件
        # file_path = os.sep.join([file_dir, filename])
        # 使用路径模块连接
        file_path = os.path.join(file_dir, filename)
        self.conn = sqlite3.connect(file_path,timeout=10,check_same_thread=False)

    def try_serial_ports(self, ports):  
        for port in ports:  
            try:  
                ser = Serial(port, 115200, timeout=1)  
                print(f"Successfully opened {port}")  
                return ser  
            except serial.serialutil.SerialException as e:  
                print(f"Failed to open {port}: {e}")  
                time.sleep(0.5)  # 等待一秒再尝试下一个端口  
        return None  
    


    def send_msg(self,message, wait_ok=False, wait_idle=True , terminator=os.linesep, com_flag = None):
        try:
            # 自动添加换行符
            if not message.endswith(terminator):
                message += terminator
            # 串口发送数据，编码为utf-8
            if com_flag == 1:
                self.serial_com1.write(message.encode('utf-8'))
            elif com_flag == 2:
                self.serial_com2.write(message.encode('utf-8'))
            
            return True
        except Exception as e:
            # 日志写入串口设备写入异常
            
            robot_logger.error(e,exc_info=True)
            return False

    def robot_type_update(self,com_flag = None):
        try:
            gcode = f"$v\r\n"
            # gcode = f'$h\r\n'
            time.sleep(0.1) 
            
            if com_flag == 1:
                self.serial_com1.write(gcode.encode('utf-8'))
                # self.send_msg(gcode,com_flag=1)
                time.sleep(1)
                data = self.serial_com1.read_all() # 从串口读取数据
                recive = str(data,"UTF-8")
                # print("receive com1:" + recive)
            time.sleep(0.01)
            
            if "Ry" in recive:
            # if "Mirobot" in recive:
                self.robot_type_number = 1
            elif "E4" in recive:
                self.robot_type_number = 2

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def robot_type_update_2(self,com_flag = None):
        try:
            gcode = f"$v\r\n"
            # gcode = f'$h\r\n'
            time.sleep(0.1) 
            
            if com_flag == 2:
                self.serial_com2.write(gcode.encode('utf-8'))
                # self.send_msg(gcode,com_flag=2)
                time.sleep(1)
                data = self.serial_com2.read_all() # 从串口读取数据
                recive = str(data,"UTF-8")
                # print("receive com2:" + recive)
            
            if "Mirobot" in recive:
                self.robot_type_number_2 = 1
            elif "E4" in recive:
                self.robot_type_number_2 = 2

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def Kata_status2json(self,data,):
        try:
            receive = str(data,"UTF-8")
            status_date = None
            status =None
            lower = receive.find('<')
            
            if lower!= -1:
                uper = receive.find('>',lower)
                if uper!=-1:
                    status_date = receive [lower+1:uper]
            
            if status_date !=None:
                status_date = status_date.split(',')
                
                status  = {

                        "state":status_date[0],
                        "angle":[
                            
                            float(status_date[5]),
                            float(status_date[6]),
                            float(status_date[7]),
                            float(status_date[1].split(':')[1]),
                            float(status_date[2]),
                            float(status_date[3]),
                            
                        ],
                        "coordinate":[
                            float(status_date[8].split(':')[1]),
                            float(status_date[9]),
                            float(status_date[10]),
                            float(status_date[11]),
                            float(status_date[12]),
                            float(status_date[13]),
                            float(status_date[4]),
                        ],
                        "pwm":[
                            float(status_date[14].split(':')[1]),
                            float(status_date[15].split(':')[1]),

                        ],
                        "mode":[
                            int(status_date[16].split(':')[1]),],
                        "robot_type":self.robot_type_number

                    }
            
            if status != None:

                self.status_global = status
                self.send_tcp_data(status)

            return status

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def Kata_status2json_2(self,data,):
        try:
            receive = str(data,"UTF-8")
            status_date = None
            status =None
            lower = receive.find('<')
            
            if lower!= -1:
                uper = receive.find('>',lower)
                if uper!=-1:
                    status_date = receive [lower+1:uper]
            
            if status_date !=None:
                status_date = status_date.split(',')
                
                status  = {

                        "state":status_date[0],
                        "angle":[
                            
                            float(status_date[5]),
                            float(status_date[6]),
                            float(status_date[7]),
                            float(status_date[1].split(':')[1]),
                            float(status_date[2]),
                            float(status_date[3]),
                            
                        ],
                        "coordinate":[
                            float(status_date[8].split(':')[1]),
                            float(status_date[9]),
                            float(status_date[10]),
                            float(status_date[11]),
                            float(status_date[12]),
                            float(status_date[13]),
                            float(status_date[4]),
                        ],
                        "pwm":[
                            float(status_date[14].split(':')[1]),
                            float(status_date[15].split(':')[1]),

                        ],
                        "mode":[
                            int(status_date[16].split(':')[1]),],
                        "robot_type":self.robot_type_number_2

                    }
            
            if status != None:
                    self.status_global_ = status
                    self.send_tcp_data_2(status)
            return status

        except Exception as e:
            robot_logger.error(e,exc_info=True)
  

    ### 将浮点数打包成32位2进制数据
    def float2bytes(self,f):
        b=struct.pack('<f',f)        
        return b[0],b[1],b[2],b[3]  

    #### 数据---TCP
    def send_tcp_data(self,data):
        try:
            j0  = data["angle"][0]
            j1  = data["angle"][1]
            j2  = data["angle"][2]
            j3  = data["angle"][3]
            j4  = data["angle"][4]
            j5  = data["angle"][5]
            j6  = data["coordinate"][6]


            # robotdata_byte0=(self.float2bytes(j0))
            # robotdata_byte1=(self.float2bytes(j1))
            # robotdata_byte2=(self.float2bytes(j2))
            # robotdata_byte3=(self.float2bytes(j3))
            # robotdata_byte4=(self.float2bytes(j4))
            # robotdata_byte5=(self.float2bytes(j5))
            # robotdata_byte6=(self.float2bytes(j6))
            
            ###将MS数据存入字典
            config.set_value("j0",j0) 
            config.set_value("j1",j1)
            config.set_value("j2",j2)
            config.set_value("j3",j3)
            config.set_value("j4",j4)
            config.set_value("j5",j5)
            config.set_value("FO[1]",value=j6)

        except Exception as e:
            robot_logger.error(e,exc_info=True)

    def send_tcp_data_2(self,data):
        try:
            j7  = data["angle"][0]
            j8  = data["angle"][1]
            j9  = data["angle"][2]
            j10  = data["angle"][3]

            # robotdata_byte7=(self.float2bytes(j7))
            # robotdata_byte8=(self.float2bytes(j8))
            # robotdata_byte9=(self.float2bytes(j9))
            # robotdata_byte10=(self.float2bytes(j10))

            config.set_value("j7",j7) 
            config.set_value("j8",j8)
            config.set_value("j9",j9)
            config.set_value("j10",j10)

        except Exception as e:
            robot_logger.error(e,exc_info=True)


    def set_robot_state(self,data):
        try :
            c = self.conn.cursor()
            state = data
            en = c.execute("SELECT EN,DATA1,DATA2 from Robot_state where ID=1")
            data_en = en.fetchall()

            if data_en is not None:
                # 检测是否有G代码要执行
                if data_en[0][0] == 1:
                    gcode = c.execute("SELECT GCODE from Robot_state where ID=1")
                    data_gcode = gcode.fetchall()
                    self.send_msg(data_gcode[0][0])
                    c.execute("UPDATE Robot_state set EN = 0 where ID=1")
                    # self.conn.commit() 
                self.rtu_slave = data_en[0][1]
                self.rut_en = data_en[0][2]  # 更新服务状态
                
                # 将机器人状态更新到数据库里。
                if state=="Idle":
                    c.execute("UPDATE Robot_state set STATE = 'Idle' where ID=1")
                elif state=="Alarm":

                    c.execute("UPDATE Robot_state set STATE = 'Alarm' where ID=1")
                elif state=="Home":

                    c.execute("UPDATE Robot_state set STATE = 'Home' where ID=1")
                elif state=="Run":

                    c.execute("UPDATE Robot_state set STATE = 'Run' where ID=1")

                self.conn.commit()
            else:
                print("No record found with ID=1 in Robot_state table.")
        except Exception as e:
            print(e)

    def set_robot_state_2(self,data):
        try :
            c = self.conn.cursor()
            state = data
            en = c.execute("SELECT EN,DATA1,DATA2 from Robot_state where ID=1")
            data_en = en.fetchall()

            if data_en is not None:
                # 检测是否有G代码要执行
                if data_en[0][0] == 1:
                    gcode = c.execute("SELECT GCODE from Robot_state where ID=1")
                    data_gcode = gcode.fetchall()
                    self.send_msg(data_gcode[0][0])
                    c.execute("UPDATE Robot_state set EN = 0 where ID=1")
                    # self.conn.commit() 
                self.rtu_slave = data_en[0][1]
                self.rut_en = data_en[0][2]  # 更新服务状态
                
                # 将机器人状态更新到数据库里。
                if state=="Idle":
                    c.execute("UPDATE Robot_state set STATE = 'Idle' where ID=1")
                elif state=="Alarm":

                    c.execute("UPDATE Robot_state set STATE = 'Alarm' where ID=1")
                elif state=="Home":

                    c.execute("UPDATE Robot_state set STATE = 'Home' where ID=1")
                elif state=="Run":

                    c.execute("UPDATE Robot_state set STATE = 'Run' where ID=1")

                self.conn.commit()
            else:
                print("No record found with ID=2 in Robot_state table.")
        except Exception as e:
            print(e)
   

    #### 六轴
    def task_com1(self,):
        type_flag_count = 0   
        while True:
            try:
                gcode = f"?"
                self.send_msg(gcode, com_flag=1)
                time.sleep(1)
                ### 从串口读取数据 
                data = self.serial_com1.read_all()
                # print(f"task_com1  Gcode ? recv data is {data}")
                ### 如果第一次建立通讯，就询问一下机器人的类型
                if data != b"":
                    type_flag_count = 0
                    ### 进来第一次询问
                    if self.robot_type_number == 0:  
                        self.robot_type_update(com_flag=1)

                else:
                    type_flag_count +=1

                    # print(type_flag_count)

                    if type_flag_count == 10:
                        self.robot_type_number = 0
                        type_flag_count = 0
                    continue

                if data != b"":
                    self.Kata_status2json(data)
                    self.set_robot_state(self.status_global["state"])
            except Exception as e:
                robot_logger.error(e,exc_info=True)
    #### 四轴
    def task_com2(self,):
        type_flag_count = 0
        while True:
            try:
                gcode = f"?"
                self.send_msg(gcode, com_flag=2)
                time.sleep(1)
                ### 从串口读取数据 
                data = self.serial_com2.read_all()
                # print(f"task_com2  Gcode ? recv data is {data}")

                ### 如果第一次建立通讯，就询问一下机器人的类型
                if data != b"":
                    type_flag_count = 0
                    ### 进来第一次询问
                    if self.robot_type_number_2 == 0:  
                        self.robot_type_update_2(com_flag=2)

                else:
                    type_flag_count +=1

                    # print(type_flag_count)

                    if type_flag_count == 10:
                        self.robot_type_number_2 = 0
                        type_flag_count = 0
                    continue

                if data != b"":
                    self.Kata_status2json_2(data)
                    time.sleep(0.1)
                    self.set_robot_state_2(self.status_global_["state"])
            except Exception as e:
                robot_logger.error(e,exc_info=True)
#### 机器人Gcode运动控制指令
####
    def home(self, has_slider=False):
        '''机械臂Homing'''
        if has_slider:
            return self.home_7axis()
        else:
            print("home")
            return self.home_6axis()

    def home_1axis(self, axis_id):
        '''单轴Homing'''
        if not isinstance(axis_id, int) or not (axis_id >= 1 and axis_id <= 7):
            return False
        msg = f'$h{axis_id}'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)

    def home_6axis(self):
        '''六轴Homing'''
        msg = f'$h'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)

    def home_6axis_in_turn(self):
        '''六轴Homing, 各关节依次Homing'''
        msg = f'$hh'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)
    
    def stop(self):
        '''6轴暂停'''
        # msg = f'!'
        # self.send_msg(msg, wait_ok=False, wait_idle=False)
        # time.sleep(0.5)
        msg = f'%'
        return self.send_msg(msg, wait_ok=False, wait_idle=False)

    def home_7axis(self):
        '''七轴Homing(本体 + 滑台)'''
        msg = f'$h0'
        return self.send_msg(msg, wait_ok=False, wait_idle=True)
        
    def unlock_all_axis(self):
        '''解锁各轴锁定状态'''
        msg = 'M50'
        return self.send_msg(msg, wait_ok=True, wait_idle=True)
        
    def go_to_zero(self):
        '''回零-运动到名义上的各轴零点'''
        msg = 'M21 G90 G00 X0 Y0 Z0 A0 B0 C0 F2000'
        return self.send_msg(msg, wait_ok=True, wait_idle=True)

    def set_speed(self, speed):
        '''设置转速'''
        # 转换为整数
        speed = int(speed)
        # 检查数值范围是否合法
        if speed <= 0 or speed > 6000:
            robot_logger.error(f"Illegal movement speed {speed}")
            return False
        # 发送指令
        msg = f'F{speed}'
        return self.send_msg(msg, wait_ok=None, wait_idle=None)

    def set_hard_limit(self, enable):
        '''
        开启硬件限位
        '''
        msg = f'$21={int(enable)}'
        return self.send_msg(msg, var_command=True, wait_ok=None)

    def set_soft_limit(self, enable):
        '''开启软限位
        注: 请谨慎使用
        '''
        msg = f'$20={int(enable)}'
        return self.send_msg(msg, var_command=True, wait_ok=None)

    def format_float_value(self, value):
        if value is None:
            return value
        if isinstance(value, float):
            # 精确到小数点后两位数
            return round(value , 2)
        else:
            return value
    
    def generate_args_string(self, instruction, pairings):
        '''生成参数字符'''
        args = [f'{arg_key}{self.format_float_value(value)}' for arg_key, value in pairings.items() if value is not None]

        return ' '.join([instruction] + args)
    
    def move(self, P, speed = None ,coordinate= 1, is_relative=0,mode =0, wait_ok=None):

        if not speed:
            speed = self.default_speed
        if speed:
            speed = int(speed)

        instruction = 'M20 G90 G1'  # X{x} Y{y} Z{z} A{a} B{b} C{c} F{speed}
        m1 = "M20"             # 世界坐标系
        pairings = {'X': P[0], 'Y': P[1], 'Z': P[2], 'A': P[3], 'B': P[4], 'C': P[5], 'D':P[6], 'F': speed}
        if coordinate ==0:
            m1 = "M21"      # 轴角坐标系
        m2 = "G90"
        if is_relative:
            m2 = 'G91'       
        m3 ="G00"
        if mode == 1:
            m3 = "G01"

        instruction = f"{m1} {m2} {m3} "
        
        msg = self.generate_args_string(instruction, pairings)
        return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True)

    def circular_interpolation(self, ex, ey, radius, is_cw=True, speed=None, wait_ok=None):
        '''圆弧插补
        在XY平面上, 从当前点运动到相对坐标(ex, ey).半径为radius
        `is_cw`决定圆弧是顺时针还是逆时针.
        '''
        # 判断是否合法
        distance = math.sqrt(ex**2 + ey**2)
        if distance > (radius * 2):
            self.logger.error(f'circular interpolation error, target posi is too far')
            return False

        instruction = None
        if is_cw:
            instruction = 'M20 G91 G02'
        else:
            instruction = 'M20 G91 G03'
        
        pairings = {'X': ex, 'Y': ey, 'R': radius, 'F': speed}
        msg = self.generate_args_string(instruction, pairings)
        return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True)

    def set_door_lift_distance(self, lift_distance):
        '''设置门式轨迹规划抬起的高度'''
        msg = f"$49={lift_distance}"
        return self.send_msg(msg, wait_ok=True, wait_idle=True)

    def door_interpolation(self, x=None, y=None, z=None, a=None, b=None, c=None, speed=None, is_relative=False, wait_ok=None):
        '''门式插补'''
        instruction = 'M20 G90 G05'  # X{x} Y{y} Z{z} A{a} B{b} C{c} F{speed}
        if is_relative:
            instruction = 'M20 G91 G05'
        
        if not speed:
            speed = self.default_speed
        if speed:
            speed = int(speed)

        pairings = {'X': x, 'Y': y, 'Z': z, 'A': a, 'B': b, 'C': c, 'F': speed}
        msg = self.generate_args_string(instruction, pairings)
        return self.send_msg(msg, wait_ok=wait_ok, wait_idle=True)


server = Controler_Server()
server.dorun()